
#include <cv.h>
#include "ColorsHSV.h"


CvMat* perspective(IplImage *frame, IplImage *search, int razmer);
int find(int type, int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer);
int findRed(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer);
int findGreen(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer);
int findBlue(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer);
int findEmpty(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer);

CvMat* perspective(IplImage *frame, IplImage *search, int razmer)
{
	CvPoint2D32f frameQuad[4], searchQuad[4];
	CvMat* warp_matpix = cvCreateMat(3,3,CV_32FC1);
	search->origin = frame->origin;
	cvZero(search);
	frameQuad[0].x = 0;                 //frame Top left
	frameQuad[0].y = 0;
	frameQuad[1].x = frame->width - 1;// -4;   //frame Top right
	frameQuad[1].y = 0;
	frameQuad[2].x = 0;                 //frame Bottom left
	frameQuad[2].y = frame->height - 1;// - 6;
	frameQuad[3].x = frame->width - 1;   //frame Bot right
	frameQuad[3].y = frame->height - 1;// - 6;
	searchQuad[0].x  = 0;//search  Top left
	searchQuad[0].y  = 0;//frame->height*0.33;
	searchQuad[1].x  = search->width - 1; //search   Top right
	searchQuad[1].y  = 0;
	searchQuad[2].x  = +razmer;//search->width*razmer/100;//search   Bottom left
	searchQuad[2].y  = search->height-1;
	searchQuad[3].x  = search->width-razmer;//*(100.-razmer)/100; //search   Bot right
	searchQuad[3].y  = search->height-1;
	cvGetPerspectiveTransform(frameQuad, searchQuad, warp_matpix);
	return warp_matpix;
}

int find(int type, int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer)
{
	int ret = -1;
	switch(type)
	{
	case RED:
		ret = findRed(aX, aY, search, c, fspace, erspace, razmer);
		break;
	case GREEN:
		ret = findGreen(aX, aY, search, c, fspace, erspace, razmer);
		break;
	case BLUE:
		ret = findBlue(aX, aY, search, c, fspace, erspace, razmer);
		break;
	case EMPTY:
		ret = findEmpty(aX, aY, search, c, fspace, erspace, razmer);
		break;
	default:
		return ret;
	case BROWN:
		return ret;
	}
	return ret;
}

int findRed(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer)
{
	ColorHSV tp;//trenutni pokazivac na mesto u slici
	aX=aY=0;//IplImage, fspace, erspace, razmer
	int errorcount;
	for (int i=search->height; i>fspace; i--)
	{
		for (int j=0; j < search->width/2-fspace/2-(razmer)-(search->height-i)*(search->height/search->width); j++)
		{
			errorcount=0;
			tp=(uchar *) &search->imageData [i*search->widthStep + (search->width/2+j)*search->nChannels];

			//provrava da li je crvena...
			if (c.r(tp))
			{
				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j+s) * search->nChannels];

						if (!c.r(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j-s) * search->nChannels];

						if (!c.r(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;

				}
				if (errorcount<erspace)
				{
					aX=j;
					aY=search->height-i;
					break;
				}
			}

			if (aY)
				break;

			tp=(uchar *) &search->imageData [i*search->widthStep - (search->width/2+j)*search->nChannels];

			if (c.r(tp))
			{

				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j+s) * search->nChannels];

						if (!c.r(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j-s) * search->nChannels];

						if (!c.r(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;
				}

				if (errorcount<erspace)
				{
					aX=-j;
					aY=search->height-i;
					break;
				}
			}

		}
		if (aY)
			break;
		}
	//
	// check for this return values
	//
	if( aY == 0 ) return 0;
	return 1;
}

int findGreen(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer)
{
	ColorHSV tp;//trenutni pokazivac na mesto u slici
	aX=aY=0;//IplImage, fspace, erspace, razmer
	int errorcount;
	for (int i=search->height; i>fspace; i--)
	{
		for (int j=0; j < search->width/2-fspace/2-(razmer)-(search->height-i)*(search->height/search->width); j++)
		{
			errorcount=0;
			tp=(uchar *) &search->imageData [i*search->widthStep + (search->width/2+j)*search->nChannels];

			if (c.g(tp))
			{
				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j+s) * search->nChannels];

						if (!c.g(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j-s) * search->nChannels];

						if (!c.g(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;

				}
				if (errorcount<erspace)
				{
					aX=j;
					aY=search->height-i;
					break;
				}
			}

			if (aY)
				break;

			tp=(uchar *) &search->imageData [i*search->widthStep - (search->width/2+j)*search->nChannels];

			if (c.g(tp))
			{

				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j+s) * search->nChannels];

						if (!c.g(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j-s) * search->nChannels];

						if (!c.g(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;
				}

				if (errorcount<erspace)
				{
					aX=-j;
					aY=search->height-i;
					break;
				}
			}

		}
		if (aY)
			break;
		}
	//
	// check for this return values
	//
	if( aY == 0 ) return 0;
	return 1;
}

int findBlue(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer)
{
	ColorHSV tp;//trenutni pokazivac na mesto u slici
	aX=aY=0;//IplImage, fspace, erspace, razmer
	int errorcount;
	for (int i=search->height; i>fspace; i--)
	{
		for (int j=0; j < search->width/2-fspace/2-(razmer)-(search->height-i)*(search->height/search->width); j++)
		{
			errorcount=0;
			tp=(uchar *) &search->imageData [i*search->widthStep + (search->width/2+j)*search->nChannels];

			if (c.b(tp))
			{
				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j+s) * search->nChannels];

						if (!c.b(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j-s) * search->nChannels];

						if (!c.b(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;

				}
				if (errorcount<erspace)
				{
					aX=j;
					aY=search->height-i;
					break;
				}
			}

			if (aY)
				break;

			tp=(uchar *) &search->imageData [i*search->widthStep - (search->width/2+j)*search->nChannels];

			if (c.b(tp))
			{

				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j+s) * search->nChannels];

						if (!c.b(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j-s) * search->nChannels];

						if (!c.b(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;
				}

				if (errorcount<erspace)
				{
					aX=-j;
					aY=search->height-i;
					break;
				}
			}

		}
		if (aY)
			break;
		}
	//
	// check for this return values
	//
	if( aY == 0 ) return 0;
	return 1;
}

int findEmpty(int &aX, int &aY, IplImage* search, ColorsHSV &c, int fspace, int erspace,int razmer)
{
	ColorHSV tp;//trenutni pokazivac na mesto u slici
	aX=aY=0;//IplImage, fspace, erspace, razmer
	int errorcount;
	for (int i=search->height; i>fspace; i--)
	{
			//for (int j=0; j < search->width/2-fspace/2; j++)
		for (int j=0; j < search->width/2-fspace/2-(razmer)-(search->height-i)*(search->height/search->width); j++)
		{
			errorcount=0;
			tp=(uchar *) &search->imageData [i*search->widthStep + (search->width/2+j)*search->nChannels];


			if (!c.r(tp) && !c.g(tp) && !c.b(tp))
			{
				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j+s) * search->nChannels];
						if (c.r(tp) || c.g(tp) || c.b(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j-s) * search->nChannels];
						if (c.r(tp) || c.g(tp) || c.b(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;

				}
				if (errorcount<erspace)
				{
					aX=j;
					aY=search->height-i;
					break;
				}
			}

			if (aY)
				break;

			tp=(uchar *) &search->imageData [i*search->widthStep - (search->width/2+j)*search->nChannels];

			if (!c.r(tp) && !c.g(tp) && !c.b(tp))
			{

				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j+s) * search->nChannels];

						if (c.r(tp) || c.g(tp) || c.b(tp))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j-s) * search->nChannels];

						if (c.r(tp) || c.g(tp) || c.b(tp))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;
				}

				if (errorcount<erspace)
				{
					aX=-j;
					aY=search->height-i;
					break;
				}
			}

		}
		if (aY)
			break;
		}
	//
	// check for this return values
	//
	if( aY == 0 ) return 0;
	return 1;
}

/*
	ColorHSV tp;//trenutni pokazivac na mesto u slici
	aX=aY=0;//IplImage, fspace, erspace, razmer
	int errorcount;
	for (int i=search->height; i>fspace; i--)
	{
			//for (int j=0; j < search->width/2-fspace/2; j++)
		for (int j=0; j < search->width/2-fspace/2-(razmer)-(search->height-i)*(search->height/search->width); j++)
		{
			errorcount=0;
			tp=(uchar *) &search->imageData [i*search->widthStep + (search->width/2+j)*search->nChannels];


			//if (!c.r(tp) && !c.g(tp) && !c.b(tp))

			//provrava da li je crvena...
			if (c.r(tp, type))
			{
				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j+s) * search->nChannels];
						//if (c.r(tp) || c.g(tp) || c.b(tp))
						if (!c.r(tp, type))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep + (search->width/2+j-s) * search->nChannels];
						//if (c.r(tp) || c.g(tp) || c.b(tp))
						if (!c.r(tp, type))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;

				}
				if (errorcount<erspace)
				{
					aX=j;
					aY=search->height-i;
					break;
				}
			}

			if (aY)
				break;

			tp=(uchar *) &search->imageData [i*search->widthStep - (search->width/2+j)*search->nChannels];

			//if (!c.r(tp) && !c.g(tp) && !c.b(tp))
			if (c.r(tp, type))
			{

				for (int up=0; up<fspace; up++)
				{
					for (int s=0; s<fspace/2; s++)
					{

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j+s) * search->nChannels];

						//if (c.r(tp) || c.g(tp) || c.b(tp))
						if (!c.r(tp, type))
							errorcount++;

						tp= (uchar *)&search->imageData [(i-up)*search->widthStep - (search->width/2+j-s) * search->nChannels];

						//if (c.r(tp) || c.g(tp) || c.b(tp))
						if (!c.r(tp, type))
							errorcount++;

						if (errorcount>=erspace)
							break;
					}
					if (errorcount>=erspace)
						break;
				}

				if (errorcount<erspace)
				{
					aX=-j;
					aY=search->height-i;
					break;
				}
			}

		}
		if (aY)
			break;
		}
	//
	// check for this return values
	//
	if( aY ) return 0;
	return -1;
}

*/




